/*******************************************************************************
* Copyright 2017 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

/* Author: Ryu Woon Jung (Leon) */

#include "sync_write.h"

typedef struct
{
  uint8_t     id;
  uint16_t    data_end;
  uint8_t     data[DATA_MAX_LEN];
}DataList;

typedef struct
{
  int         data_list_length;

  uint8_t     is_param_changed;

  uint16_t    start_address;
  uint16_t    data_length;

  DataList   data_list[TOTAL_MOTOR_NUM];
}GroupData;

static GroupData groupData;

static int size()
{
  int data_num;
  int real_size = 0;

  for (data_num = 0; data_num < groupData.data_list_length; data_num++)
  {
    if (groupData.data_list[data_num].id != NOT_USED_ID)
      real_size++;
  }
  return real_size;
}

static int find(int id)
{
  int data_num;

  for (data_num = 0; data_num < groupData.data_list_length; data_num++)
  {
    if (groupData.data_list[data_num].id == id)
      break;
  }

  return data_num;
}

void groupSyncWrite(uint16_t start_address, uint16_t data_length)
{
  groupData.data_list_length = 0;
  groupData.is_param_changed = False;
  groupData.start_address = start_address;
  groupData.data_length = data_length;
  groupSyncWriteClearParam();
}

void groupSyncWriteMakeParam()
{
  int data_num, c, idx;

  if (size() == 0)
    return;

  idx = 0;
  for (data_num = 0; data_num < groupData.data_list_length; data_num++)
  {
    if (groupData.data_list[data_num].id == NOT_USED_ID)
      continue;

    packetData.data_write[idx++] = groupData.data_list[data_num].id;
    for (c = 0; c < groupData.data_length; c++)
    {
      packetData.data_write[idx++] = groupData.data_list[data_num].data[c];
    }
  }
  //Task_Port_Write(packetData.data_write, 5*groupData.data_list_length);
}

uint8_t groupSyncWriteAddParam(uint8_t id, uint32_t data, uint16_t input_length)
{
  int data_num = 0;

  if (id == NOT_USED_ID)
    return False;

  if (groupData.data_list_length != 0)
    data_num = find(id);

  if (groupData.data_list_length == data_num)
  {
    groupData.data_list_length++;

    groupData.data_list[data_num].id = id;
    groupData.data_list[data_num].data_end = 0;
  }
  else
  {
    if (groupData.data_list[data_num].data_end + input_length > groupData.data_length)
      return False;
  }

  switch (input_length)
  {
    case 1:
      groupData.data_list[data_num].data[groupData.data_list[data_num].data_end + 0] = DXL_LOBYTE(DXL_LOWORD(data));
      break;

    case 2:
      groupData.data_list[data_num].data[groupData.data_list[data_num].data_end + 0] = DXL_LOBYTE(DXL_LOWORD(data));
      groupData.data_list[data_num].data[groupData.data_list[data_num].data_end + 1] = DXL_HIBYTE(DXL_LOWORD(data));
      break;

    case 4:
      groupData.data_list[data_num].data[groupData.data_list[data_num].data_end + 0] = DXL_LOBYTE(DXL_LOWORD(data));
      groupData.data_list[data_num].data[groupData.data_list[data_num].data_end + 1] = DXL_HIBYTE(DXL_LOWORD(data));
      groupData.data_list[data_num].data[groupData.data_list[data_num].data_end + 2] = DXL_LOBYTE(DXL_HIWORD(data));
      groupData.data_list[data_num].data[groupData.data_list[data_num].data_end + 3] = DXL_HIBYTE(DXL_HIWORD(data));
      break;

    default:
      return False;
  }
  groupData.data_list[data_num].data_end = input_length;

  groupData.is_param_changed = True;
  return True;
}

void groupSyncWriteRemoveParam(uint8_t id)
{
  int data_num = find(id);

  if (data_num == groupData.data_list_length)
    return;

  if (groupData.data_list[data_num].id == NOT_USED_ID)  // NOT exist
    return;

  groupData.data_list[data_num].data_end = 0;
  groupData.data_list[data_num].id = NOT_USED_ID;
  groupData.is_param_changed = True;
}

uint8_t groupSyncWriteChangeParam(uint8_t id, uint32_t data, uint16_t input_length, uint16_t data_pos)
{
  int data_num = 0;
  if (id == NOT_USED_ID)  // NOT exist
    return False;

  data_num = find(id);

  if (data_num == groupData.data_list_length)
    return False;

  if (data_pos + input_length > groupData.data_length)
    return False;

  switch (input_length)
  {
    case 1:
      groupData.data_list[data_num].data[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
      break;

    case 2:
      groupData.data_list[data_num].data[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
      groupData.data_list[data_num].data[data_pos + 1] = DXL_HIBYTE(DXL_LOWORD(data));
      break;

    case 4:
      groupData.data_list[data_num].data[data_pos + 0] = DXL_LOBYTE(DXL_LOWORD(data));
      groupData.data_list[data_num].data[data_pos + 1] = DXL_HIBYTE(DXL_LOWORD(data));
      groupData.data_list[data_num].data[data_pos + 2] = DXL_LOBYTE(DXL_HIWORD(data));
      groupData.data_list[data_num].data[data_pos + 3] = DXL_HIBYTE(DXL_HIWORD(data));
      break;

    default:
      return False;
  }

  groupData.is_param_changed = True;
  return True;
}

void groupSyncWriteClearParam()
{
	uint32_t i;
	groupData.data_list_length = 0;
	groupData.is_param_changed = False;
	for(i=0; i<TOTAL_MOTOR_NUM; i++)
	{
		groupData.data_list[i].id = NOT_USED_ID;
	}
}

void groupSyncWriteTxPacket()
{

  if (size() == 0)
  {
  	packetData.communication_result = COMM_NOT_AVAILABLE;
  	return;
  }

  if (groupData.is_param_changed == True)
    groupSyncWriteMakeParam();

  syncWriteTxOnly(groupData.start_address, groupData.data_length, size() * (1 + groupData.data_length));
}
